#include "ros/ros.h"
#include "cvap_6D_slam/kinect_data.h"
#include <string.h>
#include <sstream>
#include <dirent.h>
#include <sys/time.h>


using namespace cvap_6D_slam;
using namespace std;

struct frame_struct {
	double timing;
	string * rgb;
	string * depth;
} frame_struct_object;

bool compare_f(frame_struct * a, frame_struct * b)
{
	return (a->timing < b->timing);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "kth_data_kinect_node");
	ros::NodeHandle n;
	ros::Publisher chatter_pub = n.advertise<kinect_data>("kinect_data", 100000);
	//string data_path = "/media/storage/RGBD/RGBD_Dataset/daily_20110816_085255/dfki-sb/data/rgbd/";
	string data_path = "/home/johane/Desktop/rgbd/";
	struct dirent *ent;
	DIR * dir = opendir (data_path.c_str());
	int files_in_dir = 0;
	if (dir != NULL) {
		while ((ent = readdir (dir)) != NULL) {files_in_dir++;}
		closedir (dir);
	}
	else {ROS_INFO("no directory to load data from: exiting!");exit(-1);}
	ROS_INFO("total %i files",files_in_dir);

	string** rgb_list   = new string*[files_in_dir];
	string** depth_list = new string*[files_in_dir];
	double * timings = new double[files_in_dir];
	for(int i = 0; i < files_in_dir; i++)
	{
		rgb_list[i] = 0;
		depth_list[i] = 0;
	}
	
	dir = opendir (data_path.c_str());
	while ((ent = readdir (dir)) != NULL) {
		string name = ent->d_name;
		//ROS_INFO("file: %s",name.c_str());
		int strlen = name.length();
		size_t pos = name.find("_");
		if((int)pos >= 0){;
			string str3 = name.substr(pos+1);
			pos = str3.find("_");
			string str4 = str3.substr(0,pos);
			istringstream buffer(str4);
			int frame_number;
			buffer >> frame_number;
			pos = name.find("rgb");
			bool is_rgb = (int)pos > 0;
			string str5;
			if(is_rgb)
			{
				rgb_list[frame_number] = new string(data_path+name);
				str5 = name.substr(name.find("rgb")+4);
			}
			else
			{
				depth_list[frame_number] = new string(data_path+name);
				str5 = name.substr(name.find("depth")+6);
			}
			pos = str5.find("_");
			string str6 = str5.substr(0,pos);
			string str7 = str5.substr(pos+1,str5.find(".")-pos-1);
			istringstream buffer2(str6);
			int timing_large;
			buffer2 >> timing_large;
			
			istringstream buffer3(str7);
			int timing_small;
			buffer3 >> timing_small;
			double timing_tmp = timing_small;
			while(timing_tmp > 1)
			{
				timing_tmp/=10;
			}
			timing_tmp+=timing_large;

			timings[frame_number] = timing_tmp;
		}
	}
	closedir (dir);
	
	frame_struct ** data = new frame_struct * [files_in_dir];
	//frame_struct frame_data;
	int true_pos = 0;
	for(int i = 0; i < files_in_dir;i++)
	{
		if(rgb_list[i]!=0)
		{
			data[true_pos] 			= new frame_struct;
			data[true_pos]->timing 	= timings[i];
			data[true_pos]->rgb 	= rgb_list[i];
			data[true_pos]->depth 	= depth_list[i];
			true_pos++;
		}
	}
	ROS_INFO("sort..");
	int nr_frames = true_pos;
	vector<frame_struct *> myvector (data, data+nr_frames);
	vector<frame_struct *>::iterator it;
	//sort (myvector.begin(), myvector.end(), compare_f);
	ros::Rate loop_rate(10);
	int c = 0;
	frame_struct * prev;
	struct timeval start, end;
	gettimeofday(&start, NULL);
	long prev_time = start.tv_sec*1000000+start.tv_usec;
	while(true)
	{	
		for (it=myvector.begin(); it!=myvector.end() && ros::ok(); ++it)
		{
			frame_struct * current = *it;
			kinect_data msg;
			msg.timing 	= current->timing;
			msg.rgb 	= *(current->rgb);
			msg.depth 	= *(current->depth);
			chatter_pub.publish(msg);
			ros::spinOnce();
			loop_rate.sleep();
			/*
			if(c == 0){
				loop_rate.sleep();
			}
			else{
		
				gettimeofday(&end, NULL);
				long current_time = end.tv_sec*1000000+end.tv_usec;
				long target_diff = (current->timing - prev->timing)*1000000.0;
				while(current_time - prev_time < target_diff)
				{
					//ROS_INFO("%ld - %ld = %ld , target: %ld",current_time,prev_time,current_time-prev_time, target_diff);
					gettimeofday(&end, NULL);
					current_time = end.tv_sec*1000000+end.tv_usec;
					loop_rate.sleep();
				}
				prev_time = current_time;
			}
			prev = current;
			c++;
			*/
		}
	}

/*	struct timeval start, end;
	gettimeofday(&start, NULL);
	long prev = start.tv_sec+1000000*start.tv_usec;
	for(int i = 0; i < nr_frames && ros::ok();i++)
	{


		chatter_pub.publish(msg);
		ros::spinOnce();
		if(i < nr_frames-1){
			gettimeofday(&end, NULL);
			long current = end.tv_sec+1000000*end.tv_usec;
			long target_diff = (timings[i+1] - timings[i])*1000000.0;
			ROS_INFO("timings: %f - %f",timings[i+1],timings[i]);
			ROS_INFO("prev: %ld, Current: %ld, target: %ld",prev, current, target_diff);
			//while(current - prev < target_diff)
			//{ 
				loop_rate.sleep();
			//}
			prev = current;
		}else{
			loop_rate.sleep();
		}
		loop_rate.sleep();
	}
	*/
	
/*	
	ros::Rate loop_rate(30);
	while (ros::ok())
	{
		kinect_data msg;
		msg.timing 	= 1337;
		msg.rgb 	= "rgb_kth";
		msg.depth 	= "depth_kth";
		chatter_pub.publish(msg);
		ros::spinOnce();
		loop_rate.sleep();
  }
*/

  return 0;
}
